#ifndef EM80_INTERFACE_H
#define EM80_INTERFACE_H

#include <string>
#include <sstream>
#include <exception>
#include <stdexcept>

#include <boost/function.hpp>
#include <boost/noncopyable.hpp>
#include <boost/scoped_ptr.hpp>
#include <boost/shared_ptr.hpp>

#include "packets.hpp"

namespace em80_driver
{
/**
 * Convenient class for setting values for digital IO pins
 */
struct DigitalIO
{
    DigitalIO()
    {
        for(unsigned int i = 0; i < 8; ++i)
        {
            values[i] = false;
            mask[i] = false;
        }
    }

    bool values[8];     /**< Digital level for pins 0 - 7 respectively. **/
    bool mask[8];       /**< Set indices to true to set a pin, false to ignore. **/
};

class CommandAdapter
{
public:
    CommandAdapter();

    void SetVelocityControl(const int16_t &left_wheel_speed, const int16_t &right_wheel_speed);
    void SetDocking(const DockingConfig::Mode::State &mode);
    void SetLEDArray(const LEDConfig::Mode::State &mode);
    void SetGPIO(const DigitalIO &gpio);
    void SetGPIO(const uint8_t &output_mask, const uint8_t &value);
    void SetADC(const ADCConfig::Control::State &control);
    void SetDAC(const DACConfig::Control::State &control, uint16_t values);
    void SetExternalPower(const DigitalIO &power_source);
    void SetExternalPower(const uint8_t &power_source);
    void SetJoystick(const JoystickMode::Mode::State &mode);
    void GetExtended();

    EM80PacketPtr          command_buffer;  //we need to reset the buffer due to the extended request

private:
    BaseControl         base_control;
    DockingConfig       docking_config;
    LEDConfig           led_config;
    GPIOConfig          gpio_config;
    ADCConfig           adc_config;
    DACConfig           dac_config;
    PowerSourceConfig   power_source_config;
    JoystickMode        joystick_mode;
    FWVersionEnquiry    fw_version_enquiry;
    DockingEnquiry      docking_enquiry;
    StatusEnquiry       status_enquiry;

    //bool request_extra = false;

    static const uint8_t base_length = sizeof(BaseControl::Data) + EM80Frame::EM80_PAYLOAD_PREFIX + 
                                       sizeof(DockingConfig::Data) + EM80Frame::EM80_PAYLOAD_PREFIX +
                                       sizeof(LEDConfig::Data) + EM80Frame::EM80_PAYLOAD_PREFIX + 
                                       sizeof(GPIOConfig::Data) + EM80Frame::EM80_PAYLOAD_PREFIX +
                                       sizeof(ADCConfig::Data) + EM80Frame::EM80_PAYLOAD_PREFIX + 
                                       sizeof(DACConfig::Data) + EM80Frame::EM80_PAYLOAD_PREFIX +
                                       sizeof(PowerSourceConfig::Data) + EM80Frame::EM80_PAYLOAD_PREFIX + 
                                       sizeof(JoystickMode::Data) + EM80Frame::EM80_PAYLOAD_PREFIX;

    static const uint8_t extend_length = sizeof(FWVersionEnquiry::Data) + EM80Frame::EM80_PAYLOAD_PREFIX +
                                         sizeof(DockingEnquiry::Data) + EM80Frame::EM80_PAYLOAD_PREFIX +
                                         sizeof(StatusEnquiry::Data) + EM80Frame::EM80_PAYLOAD_PREFIX;


};



/**
 * Class providing an interface to the EM80 controller via a serial port interface.
 */
class EM80Interface : private boost::noncopyable
{
public:

    typedef boost::function<void (EM80PayloadBaseConstPtr&)> PayloadHandlerFunction;
    typedef boost::function<void (const std::string &)> ErrorHandlerFunction;

    /**
     * Creates a EM80Interface object. Opens the serial port interface to the EM80 if @p port
     * is not empty, otherwise the serial port remains closed until connect() is called.
     *
     * @param port Address of the serial port, e.g. '/dev/ttyUSB0'.
     * @param packet_handler Function this class calls when a EM80 payload is received.
     * @param error_handler Function this class calls when an error is detected, such as a bad checksum
     *
     * @throw SerialException
     */
    EM80Interface(const std::string &port = std::string(),
                  const PayloadHandlerFunction &payload_handler = PayloadHandlerFunction(),
                  const ErrorHandlerFunction &error_handler = ErrorHandlerFunction());

    /**
     * EM80Interface destructor.
     */
    ~EM80Interface();

    /**
     * Sets / updates the function that this class calls when an EM80 payload is received.
     */
    void setPayloadHandler(const PayloadHandlerFunction &handler);

    /**
     * Sets / updates the function that this class calls when an error is detected, such as a bad
     * checksum.
     */
    void setErrorHandler(const ErrorHandlerFunction &handler);

    /**
     * Opens the serial port interface to the EM80/
     *
     * @throw SerialException
     */
    void connect(const std::string &port);

    /**
     * Close the serial port interface to the EM80.
     */
    void disconnect();

    /**
     * Gets the status of the serial interface to the EM80.
     *
     * @return Returns true if the serial port is opened, false otherwise.
     */
    bool isConnected() const;

    /**
     * Send a EM80 packet.
     */
    void sendCommand();

    /**
     * API for commands
     */
    void SetVelocityControl(const int16_t &left_wheel_speed, const int16_t &right_wheel_speed);
    void SetDocking(const DockingConfig::Mode::State &mode);
    void SetLEDArray(const LEDConfig::Mode::State &mode);
    void SetGPIO(const DigitalIO &gpio);
    void SetGPIO(const uint8_t &output_mask, const uint8_t &value);
    void SetADC(const ADCConfig::Control::State &control);
    void SetDAC(const DACConfig::Control::State &control, uint16_t values);
    void SetExternalPower(const DigitalIO &power_source);
    void SetExternalPower(const uint8_t &power_source);
    void SetJoystick(const JoystickMode::Mode::State &mode);
    void GetExtended();


private:

    CommandAdapter command;

    // Pimpl - hide serial port members from class users
    class Impl;
    boost::scoped_ptr<Impl> impl_;
};

// TODO: review
class SerialException : public std::exception
{
    //Disable copy constructors
    SerialException& operator=(const SerialException&);
    std::string e_what_;

public:
    SerialException(const char *description)
    {
        std::stringstream ss;
        ss << "SerialException " << description << " failed.";
        e_what_ = ss.str();
    }

    SerialException(const SerialException &other) : e_what_(other.e_what_){}

    virtual ~SerialException() throw() {}
    virtual const char* what() const throw()
    {
        return e_what_.c_str();
    }
};

} //namespace em80_driver
#endif
